import math
import numpy

def detect_obs(mx, my, r, C, dL, value):
    # 1.四舍五入坐标，加1变成C矩阵的下标
    mx = round(mx/dL) + 1
    my = round(my/dL) + 1
    r = round(r/dL)
    # 2.根据半径循环覆盖值，如果距离大于r则跳过
    for i in range(0,round(r)+1):
        # 2.1 以mx, my为方心，循环方形边缘
        if mx-i > 0 and mx+i <= C.shape[0] and my+i <= C.shape[1] and my-i > 0:
            # 上边
            re = numpy.argwhere(C[mx-i:mx+i, my+i] == value)
            if len(re):
                x = re[0]
                y = re[1]
                # map(list, zip(*m)) 矩阵转置   axis=1插入列   axis=0插入行
                D = numpy.insert(D, len(D), [x+(mx-i-1), y*(my+i)], axis=1)

            # 上边
            re = numpy.argwhere(C[mx - i:mx + i, my - i] == value)
            if len(re):
                x = re[0]
                y = re[1]
                D = numpy.insert(D, len(D), [x+(mx-i-1), y*(my-i)], axis=1)

            # 左边
            re = numpy.argwhere(C[mx - i, my - i:my + i] == value)
            if len(re):
                x = re[0]
                y = re[1]
                D = numpy.insert(D, len(D), [x*(mx-i), y + (my-i-1)], axis=1)

            # 右边
            re = numpy.argwhere(C[mx + i, my - i:my + i] == value)
            if len(re):
                x = re[0]
                y = re[1]
                D = numpy.insert(D, len(D), [x * (mx + i), y + (my - i - 1)], axis=1)

    # 3.如果D不为空，则找出距离最近的障碍物坐标
    dmin = 10000  # 存储最小距离
    xmin = 10000  # 保存障碍物坐标
    ymin = 10000

    if len(D):
        for i in range(0,D.shape[1]):
            d = math.sqrt((mx-D[1, i])**2 + (my - D[2, i])**2)
            if d < dmin and d < r:
                dmin = d; xmin = D[1, i]; ymin = D[2, i]

    d = numpy.array(d)-1
    xm = xmin - 1
    ym = ymin - 1
    dm = dmin

    return (d, xm, ym, dm)

